#include "Relay.h"

using namespace RafMat::SimEngine;
namespace Jano
{
Relay::Relay(double bigRange, double smallRange)
{
    if(fabs(bigRange)>0.9)
        m_bigOffset = 0.09;
    else
        m_bigOffset = fabs(bigRange)/2.0;

    if(fabs(smallRange)>0.03)
        m_smallOffset = 0.03;
    else
        m_smallOffset = fabs(smallRange)/2.0;

    m_state = 0;
    InitializeModels();
}

void Relay::InitializeModels()
{
    Polynomial polyA(2, 1);
    Polynomial polyB;

    // 0-0.15
    polyB[0] = 0.008355;
    polyA[1] = -1.6922;
    polyA[2] = 0.70054;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.15-0.25
    polyB[0] = 0.004261;
    polyA[1] = -1.7029;
    polyA[2] = 0.70718;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.25-0.35
    polyB[0] = 0.0036725;
    polyA[1] = -1.6225;
    polyA[2] = 0.6262;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.35-0.39
    polyB[0] = 0.0042159;
    polyA[1] = -1.4801;
    polyA[2] = 0.48436;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.39-0.46
    polyB[0] = 0.0028804;
    polyA[1] = -1.5721;
    polyA[2] = 0.57493;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.46-0.5
    polyB[0] = 0.020886;
    polyA[1] = -1.6282;
    polyA[2] = 0.64913;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.5-0.6
    polyB[0] = 0.012104;
    polyA[1] = -1.6362;
    polyA[2] = 0.64833;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.6-0.7
    polyB[0] = 0.0061325;
    polyA[1] = -1.6073;
    polyA[2] = 0.61345;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.7-0.78
    polyB[0] = 0.0044527;
    polyA[1] = -1.5;
    polyA[2] = 0.50449;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyA, polyB, 0));
    // 0.78-1
    Polynomial polyAa(1, 1);
    Polynomial polyBb;
    polyBb[0] = 0.0053621;
    polyAa[1] = -0.99464;
    m_PreIdentifiedModels.push_back(Jano::PreIdentifiedObject(polyAa, polyBb, 0));
}

/*double Relay::roundPV(double pv, double precision)
{
    double roundValue = pv*pow(10.0, precision);
    roundValue = round(roundValue);
    int i = pow(10, 2);
    roundValue = roundValue/pow(10.0, precision);

    return roundValue;
}*/

//Ustawienie zakresu, a zarazem offsetu, który jest jednakowy na + i - od punktu przełączenia
void Relay::setBigRange(double range)
{
    if(fabs(range)>0.9)
        m_bigOffset = 0.09;
    else
        m_bigOffset = fabs(range)/2.0;
}

//Ustawienie zakresu, a zarazem offsetu, który jest jednakowy na + i - od punktu przełączenia
void Relay::setSmallRange(double range)
{
    if(fabs(range)>0.3)
        m_smallOffset = 0.03;
    else
        m_smallOffset = fabs(range)/2.0;
}

// Implementacja meotdy wyboru na podstawie identyfikacji wykonanej w Matlabie
void Relay::SelectModel(double PV)
{
    //PV = roundPV(PV, 2.0);
    if(PV < 0.15-m_bigOffset)
    {
        m_state = 0;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.15-m_bigOffset && PV <= 0.15+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.15+m_bigOffset && PV < 0.25-m_bigOffset)
    {
        m_state = 1;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.25-m_bigOffset && PV <= 0.25+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.25+m_bigOffset && PV < 0.35-m_bigOffset)
    {
        m_state = 2;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.35-m_bigOffset && PV <= 0.35+m_smallOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.35+m_smallOffset && PV < 0.39-m_smallOffset)
    {
        m_state = 3;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.39-m_smallOffset && PV <= 0.39+m_smallOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.39+m_smallOffset && PV < 0.46-m_smallOffset)
    {
        m_state = 4;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.46-m_smallOffset && PV <= 0.46+m_smallOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.46+m_smallOffset && PV < 0.5-m_smallOffset)
    {
        m_state = 5;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.5-m_smallOffset && PV <= 0.5+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.5+m_bigOffset && PV < 0.6-m_bigOffset)
    {
        m_state = 6;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.6-m_bigOffset && PV <= 0.6+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.6+m_bigOffset && PV < 0.7-m_bigOffset)
    {
        m_state = 7;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.7-m_bigOffset && PV <= 0.7+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.7+m_bigOffset && PV < 0.78-m_smallOffset)
    {
        m_state = 8;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV >= 0.78-m_smallOffset && PV <= 0.78+m_bigOffset)
    {
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }
    else if(PV > 0.78+m_bigOffset && PV <= 1)
    {
        m_state = 9;
        m_SelectedModel = m_PreIdentifiedModels[m_state];
    }

    std::cout<<"state = "<<m_state<<" pv = "<<PV<<std::endl;
}


// Wybrany model
Jano::PreIdentifiedObject Relay::getSelectedModel()
{
    return m_SelectedModel;
}

void Relay::reset()
{
    m_state = 0;
    m_smallOffset = 0.01;
    m_bigOffset = 0.04;
}

}
